#pragma once

#include "mat4_cxx.h"

OPEN_JLIB_NS


inline Mat4s::Mat4s (f32v4 c0, f32v4 c1, f32v4 c2, f32v4 c3) noexcept : c0(c0), c1(c1), c2(c2), c3(c3) {
}

inline Mat4s::Mat4s (const Mat4s& other) noexcept : c0(other.c0), c1(other.c1), c2(other.c2), c3(other.c3) {
}

inline Mat4s::Mat4s (const Vec4s& v0, const Vec4s& v1, const Vec4s& v2, const Vec4s& v3) noexcept : c0(v0.p), c1(v1.p), c2(v2.p), c3(v3.p) {
}

inline Mat4s::Mat4s (const f32* m) noexcept {
  for (auto i = 0; i < 16; ++i)
    s[i] = m[i];
}

inline constexpr Mat4s::Mat4s (f32 s00, f32 s10, f32 s20, f32 s30, f32 s01, f32 s11, f32 s21, f32 s31, f32 s02, f32 s12, f32 s22, f32 s32, f32 s03, f32 s13, f32 s23, f32 s33) noexcept
: c0(s00, s01, s02, s03)
, c1(s10, s11, s12, s13)
, c2(s20, s21, s22, s23)
, c3(s30, s31, s32, s33) {
}


inline Mat4s& Mat4s::operator= (const Mat4s& other) noexcept {
  c0 = other.c0;
  c1 = other.c1;
  c2 = other.c2;
  c3 = other.c3;
  return *this;
}


inline f32& Mat4s::operator[] (uwl index) noexcept {
  return s[index];
}

inline f32 Mat4s::operator[] (uwl index) const noexcept {
  return s[index];
}


inline Mat4s& Mat4s::operator+= (const Mat4s& other) noexcept {
  c0 += other.c0;
  c1 += other.c1;
  c2 += other.c2;
  c3 += other.c3;
  return *this;
}

inline Mat4s& Mat4s::operator-= (const Mat4s& other) noexcept {
  c0 -= other.c0;
  c1 -= other.c1;
  c2 -= other.c2;
  c3 -= other.c3;
  return *this;
}

inline Mat4s& Mat4s::operator*= (const Mat4s& other) noexcept {
  auto C0 = operator*(other.c0);
  auto C1 = operator*(other.c1);
  auto C2 = operator*(other.c2);
  auto C3 = operator*(other.c3);
  c0 = C0;
  c1 = C1;
  c2 = C2;
  c3 = C3;
  return *this;
}


inline Mat4s& Mat4s::operator*= (f32 s) noexcept {
  c0 *= s;
  c1 *= s;
  c2 *= s;
  c3 *= s;
  return *this;
}

inline Mat4s& Mat4s::operator/= (f32 s) noexcept {
  c0 /= s;
  c1 /= s;
  c2 /= s;
  c3 /= s;
  return *this;
}


inline Vec4s Mat4s::operator* (const Vec4s& vector) const noexcept {
  return c0 * vector.x + c1 * vector.y + c2 * vector.z * c3 * vector.w;
}

inline Mat4s Mat4s::operator+ (const Mat4s& other) const noexcept {
  auto C0 = c0 + other.c0;
  auto C1 = c1 + other.c1;
  auto C2 = c2 + other.c2;
  auto C3 = c3 + other.c3;
  return {C0, C1, C2, C3};
}

inline Mat4s Mat4s::operator- (const Mat4s& other) const noexcept {
  auto C0 = c0 - other.c0;
  auto C1 = c1 - other.c1;
  auto C2 = c2 - other.c2;
  auto C3 = c3 - other.c3;
  return {C0, C1, C2, C3};
}

inline Mat4s Mat4s::operator* (const Mat4s& other) const noexcept {
  auto C0 = operator*(other.c0);
  auto C1 = operator*(other.c1);
  auto C2 = operator*(other.c2);
  auto C3 = operator*(other.c3);
  return {C0, C1, C2, C3};
}


inline Mat4s Mat4s::operator* (f32 scalar) const noexcept {
  auto C0 = c0 * scalar;
  auto C1 = c1 * scalar;
  auto C2 = c2 * scalar;
  auto C3 = c3 * scalar;
  return {C0, C1, C2, C3};
}

inline Mat4s Mat4s::operator/ (f32 scalar) const noexcept {
  auto C0 = c0 / scalar;
  auto C1 = c1 / scalar;
  auto C2 = c2 / scalar;
  auto C3 = c3 / scalar;
  return {C0, C1, C2, C3};
}


inline f32 Mat4s::operator* () const noexcept {
  auto a0 = c0.x * c1.y - c0.y * c1.x;
  auto a1 = c0.x * c1.z - c0.z * c1.x;
  auto a2 = c0.x * c1.w - c0.w * c1.x;
  auto a4 = c0.y * c1.w - c0.w * c1.y;
  auto b0 = c2.z * c3.w - c2.w * c3.z;
  auto b1 = c2.y * c3.w - c2.w * c3.y;
  auto b2 = c2.y * c3.z - c2.z * c3.y;
  auto b4 = c2.x * c3.z - c2.z * c3.x;
  auto a3 = c0.y * c1.z - c0.z * c1.y;
  auto a5 = c0.z * c1.w - c0.w * c1.z;
  auto b3 = c2.x * c3.w - c2.w * c3.x;
  auto b5 = c2.x * c3.y - c2.y * c3.x;
  return (a0 * b0 - a1 * b1) + (a2 * b2 - a4 * b4) + (a3 * b3 + a5 * b5);
}

inline Mat4s Mat4s::operator- () const noexcept {
  return {-c0, -c1, -c2, -c3};
}

inline Mat4s Mat4s::operator~ () const noexcept {
  return {
    c0.x, c1.x, c2.x, c3.x,
    c0.y, c1.y, c2.y, c3.y,
    c0.z, c1.z, c2.z, c3.z,
    c0.w, c1.w, c2.w, c3.w,
  };
}

inline Mat4s Mat4s::operator! () const noexcept {
  auto a0 = c0.x * c1.y - c0.y * c1.x;
  auto a1 = c0.x * c1.z - c0.z * c1.x;
  auto a2 = c0.x * c1.w - c0.w * c1.x;
  auto a4 = c0.y * c1.w - c0.w * c1.y;
  auto b0 = c2.z * c3.w - c2.w * c3.z;
  auto b1 = c2.y * c3.w - c2.w * c3.y;
  auto b2 = c2.y * c3.z - c2.z * c3.y;
  auto b4 = c2.x * c3.z - c2.z * c3.x;
  auto a3 = c0.y * c1.z - c0.z * c1.y;
  auto a5 = c0.z * c1.w - c0.w * c1.z;
  auto b3 = c2.x * c3.w - c2.w * c3.x;
  auto b5 = c2.x * c3.y - c2.y * c3.x;
  auto det = (a0 * b0 - a1 * b1) + (a2 * b2 - a4 * b4) + (a3 * b3 + a5 * b5);
  auto rdet = 1.0F / det;
  auto t00 = rdet * ( c1.y * b0 - c1.z * b1 + c1.w * b2);
  auto t01 = rdet * (-c0.y * b0 + c0.z * b1 - c0.w * b2);
  auto t02 = rdet * ( c3.y * a5 - c3.z * a4 + c3.w * a3);
  auto t03 = rdet * (-c2.y * a5 + c2.z * a4 - c2.w * a3);
  auto t10 = rdet * (-c1.x * b0 + c1.z * b3 - c1.w * b4);
  auto t11 = rdet * ( c0.x * b0 - c0.z * b3 + c0.w * b4);
  auto t12 = rdet * (-c3.x * a5 + c3.z * a2 - c3.w * a1);
  auto t13 = rdet * ( c2.x * a5 - c2.z * a2 + c2.w * a1);
  auto t20 = rdet * ( c1.x * b1 - c1.y * b3 + c1.w * b5);
  auto t21 = rdet * (-c0.x * b1 + c0.y * b3 - c0.w * b5);
  auto t22 = rdet * ( c3.x * a4 - c3.y * a2 + c3.w * a0);
  auto t23 = rdet * (-c2.x * a4 + c2.y * a2 - c2.w * a0);
  auto t30 = rdet * (-c1.x * b2 + c1.y * b4 - c1.z * b5);
  auto t31 = rdet * ( c0.x * b2 - c0.y * b4 + c0.z * b5);
  auto t32 = rdet * (-c3.x * a3 + c3.y * a1 - c3.z * a0);
  auto t33 = rdet * ( c2.x * a3 - c2.y * a1 + c2.z * a0);
  return {
    t00, t10, t20, t30,
    t01, t11, t21, t31,
    t02, t12, t22, t32,
    t03, t13, t23, t33,
  };
}


bool Mat4s::inverse (f32 threshold) noexcept {
  auto a0 = c0.x * c1.y - c0.y * c1.x;
  auto a1 = c0.x * c1.z - c0.z * c1.x;
  auto a2 = c0.x * c1.w - c0.w * c1.x;
  auto a4 = c0.y * c1.w - c0.w * c1.y;
  auto b0 = c2.z * c3.w - c2.w * c3.z;
  auto b1 = c2.y * c3.w - c2.w * c3.y;
  auto b2 = c2.y * c3.z - c2.z * c3.y;
  auto b4 = c2.x * c3.z - c2.z * c3.x;
  auto a3 = c0.y * c1.z - c0.z * c1.y;
  auto a5 = c0.z * c1.w - c0.w * c1.z;
  auto b3 = c2.x * c3.w - c2.w * c3.x;
  auto b5 = c2.x * c3.y - c2.y * c3.x;
  auto det = (a0 * b0 - a1 * b1) + (a2 * b2 - a4 * b4) + (a3 * b3 + a5 * b5);
  if (fabsf(det) < threshold)
    return false;
  auto rdet = 1.0F / det;
  auto t00 = rdet * ( c1.y * b0 - c1.z * b1 + c1.w * b2);
  auto t01 = rdet * (-c0.y * b0 + c0.z * b1 - c0.w * b2);
  auto t02 = rdet * ( c3.y * a5 - c3.z * a4 + c3.w * a3);
  auto t03 = rdet * (-c2.y * a5 + c2.z * a4 - c2.w * a3);
  auto t10 = rdet * (-c1.x * b0 + c1.z * b3 - c1.w * b4);
  auto t11 = rdet * ( c0.x * b0 - c0.z * b3 + c0.w * b4);
  auto t12 = rdet * (-c3.x * a5 + c3.z * a2 - c3.w * a1);
  auto t13 = rdet * ( c2.x * a5 - c2.z * a2 + c2.w * a1);
  auto t20 = rdet * ( c1.x * b1 - c1.y * b3 + c1.w * b5);
  auto t21 = rdet * (-c0.x * b1 + c0.y * b3 - c0.w * b5);
  auto t22 = rdet * ( c3.x * a4 - c3.y * a2 + c3.w * a0);
  auto t23 = rdet * (-c2.x * a4 + c2.y * a2 - c2.w * a0);
  auto t30 = rdet * (-c1.x * b2 + c1.y * b4 - c1.z * b5);
  auto t31 = rdet * ( c0.x * b2 - c0.y * b4 + c0.z * b5);
  auto t32 = rdet * (-c3.x * a3 + c3.y * a1 - c3.z * a0);
  auto t33 = rdet * ( c2.x * a3 - c2.y * a1 + c2.z * a0);
  c0 = {t00, t01, t02, t03};
  c1 = {t10, t11, t12, t13};
  c2 = {t20, t21, t22, t23};
  c3 = {t30, t31, t32, t33};
  return true;
}


inline Mat4s Mat4s::zero () noexcept {
  return {
    0.0F, 0.0F, 0.0F, 0.0F,
    0.0F, 0.0F, 0.0F, 0.0F,
    0.0F, 0.0F, 0.0F, 0.0F,
    0.0F, 0.0F, 0.0F, 0.0F,
  };
}

inline Mat4s Mat4s::identity () noexcept {
  return {
    1.0F, 0.0F, 0.0F, 0.0F,
    0.0F, 1.0F, 0.0F, 0.0F,
    0.0F, 0.0F, 1.0F, 0.0F,
    0.0F, 0.0F, 0.0F, 1.0F,
  };
}

inline Mat4s Mat4s::translate (const Vec3s& p) noexcept {
  return {
    1.0F, 0.0F, 0.0F,  p.x,
    0.0F, 1.0F, 0.0F,  p.y,
    0.0F, 0.0F, 1.0F,  p.z,
    0.0F, 0.0F, 0.0F, 1.0F,
  };
}

inline Mat4s Mat4s::rotatex (f32 a) noexcept {
  auto sin = sinf(a);
  auto cos = sinf(a);
  return {
    1.0F, 0.0F, 0.0F, 0.0F,
    0.0F,  cos, -sin, 0.0F,
    0.0F,  sin,  cos, 0.0F,
    0.0F, 0.0F, 0.0F, 1.0F,
  };
}

inline Mat4s Mat4s::rotatey (f32 a) noexcept {
  auto sin = sinf(a);
  auto cos = sinf(a);
  return {
     cos, 0.0F,  sin, 0.0F,
    0.0F, 1.0F, 0.0F, 0.0F,
    -sin, 0.0F,  cos, 0.0F,
    0.0F, 0.0F, 0.0F, 1.0F,
  };
}

inline Mat4s Mat4s::rotatez (f32 a) noexcept {
  auto sin = sinf(a);
  auto cos = sinf(a);
  return {
     cos, -sin, 0.0F, 0.0F,
     sin,  cos, 0.0F, 0.0F,
    0.0F, 0.0F, 1.0F, 0.0F,
    0.0F, 0.0F, 0.0F, 1.0F,
  };
}

inline Mat4s Mat4s::rotatexp (f32 a, const Vec3s& p) noexcept {
  auto sin = sinf(a);
  auto cos = sinf(a);
  auto oy = p.y - p.y * cos + p.z * sin;
  auto oz = p.z - p.y * sin - p.z * cos;
  return {
    1.0F, 0.0F, 0.0F, 0.0F,
    0.0F,  cos, -sin,   oy,
    0.0F,  sin,  cos,   oz,
    0.0F, 0.0F, 0.0F, 1.0F,
  };
}

inline Mat4s Mat4s::rotateyp (f32 a, const Vec3s& p) noexcept {
  auto sin = sinf(a);
  auto cos = sinf(a);
  auto ox = p.x - p.x * cos - p.z * sin;
  auto oz = p.z + p.x * sin - p.z * cos;
  return {
     cos, 0.0F,  sin,   ox,
    0.0F, 1.0F, 0.0F, 0.0F,
    -sin, 0.0F,  cos,   oz,
    0.0F, 0.0F, 0.0F, 1.0F,
  };
}

inline Mat4s Mat4s::rotatezp (f32 a, const Vec3s& p) noexcept {
  auto sin = sinf(a);
  auto cos = sinf(a);
  auto ox = p.x - p.x * cos + p.y * sin;
  auto oy = p.y - p.x * sin - p.y * cos;
  return {
     cos, -sin, 0.0F,   ox,
     sin,  cos, 0.0F,   oy,
    0.0F, 0.0F, 1.0F, 0.0F,
    0.0F, 0.0F, 0.0F, 1.0F,
  };
}

inline Mat4s Mat4s::rotatev (f32 a, const Vec3s& v) noexcept {
  auto sin = sinf(a);
  auto cos = cosf(a);
  auto c = 1.0F - cos;
  auto x = v.x, y = v.y, z = v.z;
  auto cx = c * x, cy = c * y, cz = c * z;
  auto cxx = cx * x, cxy = cx * y, cxz = cx * z;
  auto cyy = cy * y, cyz = cy * z;
  auto czz = cz * z;
  auto sx = sin * x, sy = sin * y, sz = sin * z;
  return {
    cos+cxx,  cxy+sz,  cxz-sy, 0.0F,
     cxy-sz, cos+cyy,  cyz+sx, 0.0F,
     cxz+sy,  cyz-sx, cos+czz, 0.0F,
       0.0F,    0.0F,    0.0F, 1.0F,
  };
}

inline Mat4s Mat4s::rotatevp (f32 a, const Vec3s& v, const Vec3s& p) noexcept {
  auto sin = sinf(a);
  auto cos = cosf(a);
  auto c= 1.0F - cos;
  auto x = v.x, y = v.y, z = v.z;
  auto cx = c * x, cy = c * y, cz = c * z;
  auto cxx = cx * x, cxy = cx * y, cxz = cx * z;
  auto cyy = cy * y, cyz = cy * z;
  auto czz = cz * z;
  auto sx = sin * x, sy = sin * y, sz = sin * z;
  auto c0 = Vec4s(cos+cxx,  cxy+sz,  cxz-sy, 0.0F);
  auto c1 = Vec4s( cxy-sz, cos+cyy,  cyz+sx, 0.0F);
  auto c2 = Vec4s( cxz+sy,  cyz-sx, cos+czz, 0.0F);
  auto ox = p.x - p.x * c0.x - p.y * c1.x - p.z * c2.x;
  auto oy = p.y - p.x * c0.y - p.y * c1.y - p.z * c2.y;
  auto oz = p.z - p.x * c0.z - p.y * c1.z - p.z * c2.z;
  auto c3 = Vec4s(ox, oy, oz, 1.0F);
  return {c0, c1, c2, c3};
}

inline Mat4s Mat4s::scale (const Vec3s& t) noexcept {
  return {
     t.x, 0.0F, 0.0F, 0.0F,
    0.0F,  t.y, 0.0F, 0.0F,
    0.0F, 0.0F,  t.z, 0.0F,
    0.0F, 0.0F, 0.0F, 1.0F,
  };
}

inline Mat4s Mat4s::scalep (const Vec3s& t, const Vec3s& p) noexcept {
  auto ox = p.x - p.x * t.x;
  auto oy = p.y - p.y * t.y;
  auto oz = p.z - p.z * t.z;
  return {
    t.x, 0.0F, 0.0F, 0.0F,
    0.0F,  t.y, 0.0F, 0.0F,
    0.0F, 0.0F,  t.z, 0.0F,
      ox,   oy,   oz, 1.0F,
  };
}

inline Mat4s Mat4s::scalev (const f32 t, const Vec3s& v) noexcept {
  auto f = t - 1.0F;
  auto x = v.x, y = v.y, z = v.z;
  auto fx = f * x, fy = f * y, fz = f * z;
  auto fxx = fx * x, fxy = fx * y, fxz = fx * z;
  auto fyy = fy * y, fyz = fy * z;
  auto fzz = fz * z;
  return {
    1.0F+fxx,      fxy,      fxz, 0.0F,
         fxy, 1.0F+fyy,      fyz, 0.0F,
         fxz,      fyz, 1.0F+fzz, 0.0F,
        0.0F,     0.0F,     0.0F, 1.0F,
  };
}

inline Mat4s Mat4s::scalevp (const f32 t, const Vec3s& v, const Vec3s& p) noexcept {
  auto f = t - 1.0F;
  auto x = v.x, y = v.y, z = v.z;
  auto w = -x * p.x - y * p.y - z * p.z;
  auto fx = f * x, fy = f * y, fz = f * z;
  auto fxx = fx * x, fxy = fx * y, fxz = fx * z;
  auto fyy = fy * y, fyz = fy * z;
  auto fzz = fz * z;
  auto fwx = fx * w, fwy = fy * w, fwz = fz * w;
  return {
    1.0F+fxx,      fxy,      fxz, 0.0F,
         fxy, 1.0F+fyy,      fyz, 0.0F,
         fxz,      fyz, 1.0F+fzz, 0.0F,
         fwx,      fwy,      fwz, 1.0F,
  };
}

inline Mat4s Mat4s::skewx (f32 ay, f32 az) noexcept {

}
inline Mat4s Mat4s::skewy (f32 az, f32 ax) noexcept;
inline Mat4s Mat4s::skewz (f32 ax, f32 ay) noexcept;
inline Mat4s Mat4s::skewxp (f32 ay, f32 az, const Vec3s& p) noexcept;
inline Mat4s Mat4s::skewyp (f32 az, f32 ax, const Vec3s& p) noexcept;
inline Mat4s Mat4s::skewzp (f32 ax, f32 ay, const Vec3s& p) noexcept;
inline Mat4s Mat4s::skewv (const Vec3s& s, const Vec3s& d) noexcept;
inline Mat4s Mat4s::skewvp (const Vec3s& s, const Vec3s& d, const Vec3s& p) noexcept;
inline Mat4s Mat4s::lookat (const Vec3s& s, const Vec3s& d, const Vec3s& u) noexcept;
inline Mat4s Mat4s::orthogonal (const Box3s& frustum) noexcept;
inline Mat4s Mat4s::perspective (f32 fovy, f32 aspect, f32 near, f32 far) noexcept;


CLOSE_JLIB_NS